#!/usr/bin/python3
# -*- coding: UTF-8 -*-
import numpy as np
# import cv2 as cv
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from typing import List
from shapely.geometry import Polygon as Rectangle
import pandas as pd
import sys
sys.path.append(sys.path[0]+"/../")

# csv_file = sys.path[0] + "/../merged.csv"
# csv_file = sys.path[0] + "/../optimize_split.csv"
# csv_file = sys.path[0] + "/../target.csv"
csv_file = sys.path[0] + "/../split.csv"


class Obstacle:
    def __init__(self, center_x, center_y, length, width, heading, counter_time=0, classification=0, score=0, height=0,
                 obj_longitude=0, obj_latitude=0):
        self.counter_time = counter_time
        self.classification = classification
        self.score = score
        self.height = height
        self.obj_longitude = obj_longitude
        self.obj_latitude = obj_latitude
        self.center_x = center_x
        self.center_y = center_y
        self.length = length
        self.width = width
        self.heading = heading


class Matplotlib_vis:
    def __init__(self, width, height):
        self.img_width = width
        self.img_height = height
        self.vanus = np.zeros((self.img_height, self.img_width, 3), np.uint8)
        self.vanus[:] = 255

    def display(self):
        plt.show()

    def calculate_rectangle(self, obs: Obstacle) -> Rectangle:
        """根据矩形长宽生成四个顶点
        默认矩形长平行与正北方向
        TODO Python 会修改原list中的obs，不用返回值

        Args:
        obs (Obstacle): 障碍物信息
        Rectangle: 四个顶点组成的矩形，从左上角按顺时针方向生成
        """
        from utils.tools import rotatation_matrix, transform_martix
        from utils.latlontomap import do_transform
        rect = Rectangle([
            [-obs.width/2, obs.length/2],
            [obs.width/2, obs.length/2],
            [obs.width/2, -obs.length/2],
            [-obs.width/2, -obs.length/2]])
        new_rect = []
        rect_rotat = rotatation_matrix(obs.heading)
        rect_trans = transform_martix(obs.center_x, obs.center_y)
        for i in range(4):
            rect_point = np.array(rect.exterior.coords[i])
            rect_point = do_transform(p=rect_point, r=rect_rotat, t=rect_trans)
            new_rect.append(rect_point)
        rectangle = Rectangle(new_rect)

        return rectangle

    def padding_vanus(self, data: List[Obstacle]):
        plt.imshow(self.vanus)
        ax = plt.gca()
        ax.yaxis.set_major_locator(plt.MultipleLocator(4.43))
        ax.xaxis.set_major_locator(plt.MultipleLocator(1.59))
        # ax.grid(which='major', axis='both', color='orangered', linewidth=0.75)
        plt.xticks(rotation=270)
        for obs in data:
            # rect = patches.Rectangle((data[i].center_x,data[i].center_y),\
            #         data[i].width, data[i].length,data[i].heading,fill = False)
            # rect
            rect = self.calculate_rectangle(obs)
            points = rect.exterior
            points = np.array(points)
            rect = patches.Polygon(points, fill=False)
            ax.add_patch(rect)
        self.display()


if __name__ == '__main__':
    test = Matplotlib_vis(105, 105)
    data = pd.read_csv(csv_file, usecols=[
                       'center_x', 'center_y', 'length', 'width', 'heading'])
    data = np.array(data)
    list_test = []
    for i in range(data.shape[0]):
        obj = Obstacle(data[i][0]+46, data[i][1]+41,
                       data[i][2], data[i][3], data[i][4])
        # obj = Obstacle(50,100,10,20,0)
        # pretty_print(obj)
        list_test.append(obj)

    test.padding_vanus(list_test)
